#include <robot/robot.h>

#include <robot/nao/cnaoclient.h>
#include <blackbox/connect.h>

namespace buola { namespace robot { namespace nao {

CNaoClient::CNaoClient()
    :   BBox("nao client")
    ,   iJoints(this,"joints in",26)
{
}

CNaoClient::~CNaoClient()
{
}

void CNaoClient::Init(const blackbox::CBoxProxy &pBox)
{
    mNao.reset(new blackbox::CBoxProxy(pBox));
    bb_connect(pBox["joints out"],iJoints);
}

void CNaoClient::GetPose(mat::CVecNd<26> &pValues)
{
    PullAtomicInputs();
    std::vector<double> lAngles;
    iJoints.GetData(lAngles);
    for(int i=0;i<26;i++)
        pValues[i]=lAngles[i];
}

void CNaoClient::SetGoal(const mat::CVecNd<26> &pJoints)
{
    mNao->Call<void(mat::CVecNd<26>)>("set goal",pJoints);
}

void CNaoClient::Speak()
{
    mNao->Call<void()>("speak");
}

void CNaoClient::Walk()
{
    mNao->Call<void()>("walk");
}

void CNaoClient::Posture(const std::string &pName)
{
    mNao->Call<void(const std::string&)>("posture",pName);
}

void CNaoClient::Relax()
{
    mNao->Call<void()>("relax");
}

/*namespace nao*/ } /*namespace robot*/ } /*namespace buola*/ }
